
// ROS node for Magabot
// (C) 2013 Rodrigo Ventura

#include <Wire.h>
#include "Magabot.h"

#include <ros.h>
#include <magabot/Status.h>
#include <magabot/Actuation.h>

#define N_SONARS 5
#define N_BUMPERS 4
#define N_IR 3

Magabot robot;

int odometry[2] = {0, 0};
unsigned long period = 100; // in msecs
unsigned long next = 0;

ros::NodeHandle n;
magabot::Status status_msg;
ros::Publisher status_pub("/magabot/status", &status_msg);

void update_status(magabot::Status *msg) {
  // Battery
  robot.readBattery();
  msg->battery = robot.batteryRead;
  // Sonars
  robot.readSonars();
  for (int i=0 ; i<N_SONARS ; i++) {
    msg->sonars[i] = robot.sonarRead[i].read();
  }
  // Bumpers
  robot.readBumpers();
  for (int i=0 ; i<N_BUMPERS ; i++) {
    msg->bumpers[i] = robot.bumperRead[i];
  }
  // IR
  robot.readIR();
  for (int i=0 ; i<N_IR ; i++) {
    msg->ir[i] = robot.irRead[i];
  }
  // Odometry
  robot.readClicks();
  odometry[0] += robot.leftClicks;
  odometry[1] += robot.rightClicks;
  msg->odometry[0] = odometry[0];
  msg->odometry[1] = odometry[1];
}

void callback_actuation(const magabot::Actuation &msg) {
  if (msg.set_velocity) {
    robot.actuateMotors(msg.velocity[0], msg.velocity[1]);
  }
  if (msg.set_leds) {
    robot.actuateLEDs(msg.red, msg.green, msg.blue);
  }
}

ros::Subscriber<magabot::Actuation> actuation_sub("/magabot/actuation", callback_actuation);

void setup() {
  n.initNode();
  n.advertise(status_pub);
  n.subscribe(actuation_sub);
  next = millis();
}

void loop() {
  if (millis() >= next) {
    update_status(&status_msg);
    status_pub.publish(&status_msg);
    next += period;
    n.spinOnce();
    digitalWrite(13, !digitalRead(13));
  }
  robot.update();
}

// EOF

